#include "Acc_Calc.h"
#include "stm32f10x_conf.h"
#include <math.h>

double dt = 0.02;				// Polling interval (1/50 Hz = 0.02 s) 
double RC = 0.019894368;			// Time Constant for a 8 Hz cutoff frequency (1/(2*pi*f) = 0.019894368 s)

int i;
double Acc_x, Acc_y, Acc_z;

void atanGenerator(double *out)
{
	int size = ATAN_SIZE;
	int i;
	for (i=0; i<size; i++)
	{
	 	out[i] = atan(((double)i)/4)*180/PI;
	}
}

//
//double arctan(double input, double [] table)
//{
//	
//
// 	
//}
//
///**
// *  Calculates the yaw, pitch and roll angles from the accelerometer x,y,z values passed to it.
// *	The formula used is the 3D tilt calculation formula i.e. Angle alpha = arctan(ax / sqrt(ay^2 + az^2))
// *
// */
//void calculate_gyro (s16 *acceleration, double *out) {
//
//	Acc_x = (double) (acceleration[0] + off_x);
//	Acc_y = (double) (acceleration[1] + off_y);
//	Acc_z = (double) (acceleration[2] + off_z);
//
//	out[0] = TO_DEG*atan2(Acc_x, fabs(Acc_y));	//Yaw angle
//	out[1] = TO_DEG*atan2(Acc_y, fabs(Acc_z));	//Roll angle
//	out[2] = TO_DEG*atan2(Acc_x, fabs(Acc_z));	//Pitch angle
//}
//
///**
// *  Calculates the yaw, pitch and roll angles from the accelerometer x,y,z values passed to it.
// *	The formula used is the 3D tilt calculation formula i.e. Angle alpha = arctan(ax / sqrt(ay^2 + az^2))
// *  as above, but this time using a lookup table for arctan to improve performance.
// *
// */
//void lookup_gyro (s16 *acceleration, double *out) {
// 	
//	Acc_x = (double) (acceleration[0] + off_x);
//	Acc_y = (double) (acceleration[1] + off_y);
//	Acc_z = (double) (acceleration[2] + off_z);
//
//	out[0] = arctan(Acc_x, Acc_y);	//Yaw angle
//	out[1] = arctan(Acc_y, Acc_z);	//Roll angle
//	out[2] = arctan(Acc_x, Acc_z);	//Pitch angle	
//}
//
///**
// *  Applies a recursive low pass filter to our data, with cutoff frequency of 12 Hz. 
// */
//void lowpass_filter(s16 x[FILTSIZE], s16 y[FILTSIZE], int j) {
//	int iprev;
//	double coeff = dt/(RC + dt);
//	
//	/*
//	i goes from j to 10+j
//	y = yprev + coeff* (x - yprev);
//	We're only reading the filtered value at j, no need to fill any other value
//	*/
//	i = j;
//	iprev = i-1;
//	if (iprev < 0){
//		iprev = FILTSIZE-1;
//	}
//	
//	y[i] = y[iprev] + coeff*(x[i] - y[iprev]);
//	
//	//}
//}
//
///**
// *  Applies a 3-value mean filter to our incoming data, to act as a simple low-pass filter.
// *  "Deprecated," in that we have now settled on a proper low pass filter over this mean filter.
// */
//void mean_filter (s16 acc_data[3][3], s16 acc_filtered[3]){
//	
//	acc_filtered[0] = (acc_data[0][0] + acc_data[1][0] + acc_data[2][0])/3;
//	acc_filtered[1] = (acc_data[0][1] + acc_data[1][1] + acc_data[2][1])/3;
//	acc_filtered[2] = (acc_data[0][2] + acc_data[1][2] + acc_data[2][2])/3;
//}
//
